#pragma once


#include "math/Vector.hpp"
#include "math/Point.hpp"
#include <cmath>
#include <initializer_list>


struct Matrix
{
    const static int MATRIX_SIZE = 4;

    Matrix()
    {
        for (int row = 0; row < MATRIX_SIZE; row++)
            for (int col = 0; col < MATRIX_SIZE; col++)
                data[row][col] = row == col ? 1.0f : 0.0f;
    }

    Matrix(std::initializer_list<double> values)
    {
        if (values.size() >= MATRIX_SIZE * MATRIX_SIZE)
        {
            auto it{values.begin()};
            for (int row = 0; row < MATRIX_SIZE; row++)
                for (int col = 0; col < MATRIX_SIZE; col++)
                    data[row][col] = *it++;
        }
    }

    Matrix(const Vector& col0, const Vector& col1, const Vector& col2)
    {
        data[0][0] = col0.x();
        data[1][0] = col0.y();
        data[2][0] = col0.z();
        data[3][0] = 0.0f;

        data[0][1] = col1.x();
        data[1][1] = col1.y();
        data[2][1] = col1.z();
        data[3][1] = 0.0f;

        data[0][2] = col2.x();
        data[1][2] = col2.y();
        data[2][2] = col2.z();
        data[3][2] = 0.0f;

        data[0][3] = 0.0f;
        data[1][3] = 0.0f;
        data[2][3] = 0.0f;
        data[3][3] = 1.0f;
    }

    Matrix(const Matrix &rhs)
    {
        *this = rhs;
    }

    Matrix& operator=(const Matrix &rhs)
    {
        for (int row = 0; row < MATRIX_SIZE; row++)
            for (int col = 0; col < MATRIX_SIZE; col++)
                data[row][col] = rhs.data[row][col];
        return *this;
    }

    Matrix operator*(const Matrix& rhs) const
    {
        Matrix result;
        for (int row = 0; row < MATRIX_SIZE; row++)
            for (int col = 0; col < MATRIX_SIZE; col++)
            {
                result.data[row][col] = 0.0f;
                for (int i = 0; i < MATRIX_SIZE; i++)
                    result.data[row][col] += data[row][i] * rhs.data[i][col];
            }
        return result;
    }

    Vector operator*(const Vector& v) const
    {
        return Vector(
            data[0][0] * v.x() + data[0][1] * v.y() + data[0][2] * v.z(),
            data[1][0] * v.x() + data[1][1] * v.y() + data[1][2] * v.z(),
            data[2][0] * v.x() + data[2][1] * v.y() + data[2][2] * v.z()
        );
    }

    Point operator*(const Point& p) const
    {
        double value = data[3][0] * p.x + data[3][1] * p.y + data[3][2] * p.z + data[3][3];
        return Point(
            (data[0][0] * p.x + data[0][1] * p.y + data[0][2] * p.z) / value,
            (data[1][0] * p.x + data[1][1] * p.y + data[1][2] * p.z) / value,
            (data[2][0] * p.x + data[2][1] * p.y + data[2][2] * p.z) / value
        );
    }

    void swapRow(int row0, int row1)
    {
        for (int col = 0; col < MATRIX_SIZE; col++)
        {
            double value = data[row0][col];
            data[row0][col] = data[row1][col];
            data[row1][col] = value;
        }
    }

    void mergeRow(int row0, int row1, double x)
    {
        for (int col = 0; col < MATRIX_SIZE; col++)
            data[row0][col] += data[row1][col] * x;
    }

    void scaleRow(int row, double scale)
    {
        for (int col = 0; col < MATRIX_SIZE; col++)
            data[row][col] *= scale;
    }

    double det() const
    {
        double det = 1.0f;
        Matrix matrix(*this);
        for (int row = 0; row < MATRIX_SIZE - 1; row++)
        {
            int maxRow = row;
            for (int rowTemp = row + 1; rowTemp < MATRIX_SIZE; rowTemp++)
                if (abs(matrix.data[rowTemp][row]) > abs(matrix.data[maxRow][row]))
                    maxRow = rowTemp;

            if (maxRow != row)
            {
                det = -det;
                matrix.swapRow(row, maxRow);
            }

            for (int row1 = row + 1; row1 < MATRIX_SIZE; row1++)
            {
                double valueTemp = -matrix.data[row1][row] / matrix.data[row][row];
                matrix.mergeRow(row1, row, valueTemp);
            }
        }

        for (int row = 0; row < MATRIX_SIZE; row++)
            det *= matrix.data[row][row];
        return det;
    }

    Matrix Inverse() const
    {
        Matrix matrix(*this);
        Matrix result(UnitMatrix());
        for (int row = 0; row < MATRIX_SIZE - 1; row++)
        {
            int maxRow = row;
            for (int rowTemp = row + 1; rowTemp < MATRIX_SIZE; rowTemp++)
                if (abs(matrix.data[rowTemp][row]) > abs(matrix.data[maxRow][row]))
                    maxRow = rowTemp;

            if (maxRow != row)
            {
                matrix.swapRow(row, maxRow);
                result.swapRow(row, maxRow);
            }

            for (int row1 = row + 1; row1 < MATRIX_SIZE; row1++)
            {
                double valueTemp = -matrix.data[row1][row] / matrix.data[row][row];
                matrix.mergeRow(row1, row, valueTemp);
                result.mergeRow(row1, row, valueTemp);
            }
        }

        for (int row = MATRIX_SIZE - 1; row > 0; row--)
        {
            for (int row1 = row - 1; row1 >= 0; row1--)
            {
                double valueTemp = -matrix.data[row1][row] / matrix.data[row][row];
                result.mergeRow(row1, row, valueTemp);
            }
        }

        for (int row = 0; row < MATRIX_SIZE; row++)
        {
            result.scaleRow(row, 1 / matrix.data[row][row]);
        }
        return result;
    }

    void transpose()
    {
        Matrix matrix(*this);
        for (int row = 0; row < MATRIX_SIZE; row++)
            for (int col = 0; col < MATRIX_SIZE; col++)
                data[row][col] = matrix.data[col][row];
    }

    Matrix getTransposed() const
    {
        Matrix matrix;
        for (int row = 0; row < MATRIX_SIZE; row++)
            for (int col = 0; col < MATRIX_SIZE; col++)
                matrix.data[row][col] = data[col][row];
        return matrix;
    }

    static const Matrix UnitMatrix()
    {
        static Matrix unitMatrix;
        return unitMatrix;
    }

    double data[MATRIX_SIZE][MATRIX_SIZE];     // row major matrix data
};
